/*
 * KdTree.h
 * RVO2 Library
 *
 * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <https://gamma.cs.unc.edu/RVO2/>
 */

import { Agent } from "./Agent";

import { Obstacle, RvoPolygon } from "./Obstacle";
import { IRVOArrayItem, RVOArray } from "./RVOArrayEx";
import { RvoPool } from "./RvoPool";
import { RVOSimulator } from "./RVOSimulator";
import { RVO_EPSILON, Vector2 } from "./Vector2";

let _v1 = new Vector2;
let _v2 = new Vector2;
let _v3 = new Vector2;
const RVO_MAX_LEAF_SIZE = 10;

class AgentTreeNode implements IRVOArrayItem {
  rvoIndex: number;

  constructor() {
    this.begin = 0;
    this.end = 0;
    this.left = 0;
    this.right = 0;
    this.maxX = 0;
    this.minX = 0;
    this.minY = 0;
    this.maxY = 0;
  }
  /**
   * @brief The beginning node number.
   */
  begin: number;

  /**
   * @brief The ending node number.
   */
  end: number;

  /**
   * @brief The left node number.
   */
  left: number;

  /**
   * @brief The right node number.
   */
  right: number;

  /**
   * @brief The maximum x-coordinate.
   */
  maxX: number;

  /**
   * @brief The maximum y-coordinate.
   */
  maxY: number;

  /**
   * @brief The minimum x-coordinate.
   */
  minX: number;

  /**
   * @brief The minimum y-coordinate.
   */
  minY: number;
}

class ObstacleTreeNode {
  constructor() {
    this.obstacle = null;
    this.left = null;
    this.right = null;
  }
  /**
   * @brief The obstacle number.
   */
  obstacle: Obstacle;

  /**
   * @brief The left obstacle tree node.
   */
  left: ObstacleTreeNode;

  /**
   * @brief The right obstacle tree node.
   */
  right: ObstacleTreeNode;
}

/**
 * @brief Defines k-D trees for agents and static obstacles in the simulation.
 */
export class KdTree {
  /**
   * @brief     Constructs a k-D tree instance.
   * @param[in] simulator The simulator instance.
   */
  constructor(simulator: RVOSimulator) {
    this.simulator_ = simulator;
    this.obstacleTree_ = null;
    this.agents_ = new RVOArray();
    this.agentTree_ = new RVOArray();
    this.isAgentsTreeDirty = true;
    this.isObstacleTreeDirty = true;
  }

  /**
   * @brief Builds an agent k-D tree.
   */
  buildAgentTree() {
    if (this.isAgentsTreeDirty) {
      this.agents_.copy(this.simulator_.agents_);
      let oldLen = this.agentTree_.length;
      this.agentTree_.length = 2 * this.agents_.length - 1;

      if (this.agentTree_.length > oldLen) {
        for (let i = oldLen, len = this.agentTree_.length; i < len; i++) {
          if (!this.agentTree_.get(i)) {
            this.agentTree_.set(i, new AgentTreeNode());
          }
        }
      }

      this.isAgentsTreeDirty = false;
    }

    if (this.agents_.length) {
      this.buildAgentTreeRecursive(0, this.agents_.length, 0);
    }
  }

  /**
   * @brief     Recursive function to build an agent k-D tree.
   * @param[in] begin The beginning agent k-D tree node.
   * @param[in] end   The ending agent k-D tree node.
   * @param[in] node  The current agent k-D tree node.
   */
  buildAgentTreeRecursive(begin: number, end: number, node: number) {
    let agentTree = this.agentTree_.get(node);
    let agent = this.agents_.get(begin);

    agentTree.begin = begin;
    agentTree.end = end;
    agentTree.minX = agentTree.maxX = agent.position_.x_;
    agentTree.minY = agentTree.maxY = agent.position_.y_;


    for (let i = begin + 1; i < end; ++i) {
      agent = this.agents_.get(i);
      agentTree.maxX = Math.max(agentTree.maxX, agent.position_.x_);
      agentTree.minX = Math.min(agentTree.minX, agent.position_.x_);
      agentTree.maxY = Math.max(agentTree.maxY, agent.position_.y_);
      agentTree.minY = Math.min(agentTree.minY, agent.position_.y_);
    }

    if (end - begin > RVO_MAX_LEAF_SIZE) {
      /* No leaf node. */
      let isVertical = agentTree.maxX - agentTree.minX >
        agentTree.maxY - agentTree.minY;
      let splitValue =
        0.5 * (isVertical ? agentTree.maxX + agentTree.minX
          : agentTree.maxY + agentTree.minY);

      let left = begin;
      let right = end;
      let temp;
      while (left < right) {
        while (left < right &&
          (isVertical ? this.agents_.get(left).position_.x_ : this.agents_.get(left).position_.y_) < splitValue) {
          ++left;
        }
        while (right > left &&
          (isVertical ? this.agents_.get(right - 1).position_.x_ : this.agents_.get(right - 1).position_.y_) >= splitValue) {
          --right;
        }

        if (left < right) {
          temp = this.agents_.get(left);
          this.agents_.set(left, this.agents_.get(right - 1), false);
          this.agents_.set(right - 1, temp, false);
          ++left;
          --right;
        }
      }

      if (left == begin) {
        ++left;
        ++right;
      }

      agentTree.left = node + 1;
      agentTree.right = node + 2 * (left - begin);

      this.buildAgentTreeRecursive(begin, left, agentTree.left);
      this.buildAgentTreeRecursive(left, end, agentTree.right);
    }
  }

  /**
   * @brief Builds an obstacle k-D tree.
   */
  buildObstacleTree() {
    if (this.isObstacleTreeDirty) {
      this.deleteObstacleTree(this.obstacleTree_);
      RvoPool.recycleFromArray(this.simulator_.obstacles_, Obstacle.pool);
      for (let i = 0, len = this.simulator_.rvoPolygon.length; i < len; i++) {
        this.simulator_.addObstacle(this.simulator_.rvoPolygon.get(i).vertices, false);
      }
      this.obstacleTree_ = this.buildObstacleTreeRecursive(this.simulator_.obstacles_);
      this.isObstacleTreeDirty = false;
    }
  }

  /**
   * @brief     Recursive function to build an obstacle k-D tree.
   * @param[in] obstacles List of obstacles from which to build the obstacle k-D
   *                      tree.
   */
  buildObstacleTreeRecursive(obstacles: RVOArray<Obstacle>): ObstacleTreeNode {
    if (obstacles.length) {
      let node = new ObstacleTreeNode();

      let optimalSplit = 0;
      let minLeft = obstacles.length;
      let minRight = obstacles.length;

      for (let i = 0; i < obstacles.length; ++i) {
        let leftSize = 0;
        let rightSize = 0;

        let obstacleI1 = obstacles.get(i);
        let obstacleI2 = obstacleI1.next_;

        /* Compute optimal split node. */
        for (let j = 0; j < obstacles.length; ++j) {
          if (i != j) {
            let obstacleJ1 = obstacles.get(j);
            let obstacleJ2 = obstacleJ1.next_;

            let j1LeftOfI = Vector2.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
            let j2LeftOfI = Vector2.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

            if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
              ++leftSize;
            } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
              ++rightSize;
            } else {
              ++leftSize;
              ++rightSize;
            }

            if (Math.max(leftSize, rightSize) >= Math.max(minLeft, minRight) &&
              Math.min(leftSize, rightSize) >= Math.min(minLeft, minRight)) {
              break;
            }
          }
        }

        if (Math.max(leftSize, rightSize) < Math.max(minLeft, minRight) ||
          Math.min(leftSize, rightSize) < Math.min(minLeft, minRight)) {
          minLeft = leftSize;
          minRight = rightSize;
          optimalSplit = i;
        }
      }

      /* Build split node. */
      let leftObstacles: RVOArray<Obstacle> = new RVOArray(minLeft);
      let rightObstacles: RVOArray<Obstacle> = new RVOArray(minRight);

      let leftCounter = 0;
      let rightCounter = 0;
      let i = optimalSplit;

      let obstacleI1 = obstacles.get(i);
      let obstacleI2 = obstacleI1.next_;

      for (let j = 0; j < obstacles.length; ++j) {
        if (i != j) {
          let obstacleJ1 = obstacles.get(j);
          let obstacleJ2 = obstacleJ1.next_;

          let j1LeftOfI = Vector2.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_);
          let j2LeftOfI = Vector2.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_);

          if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
            leftObstacles.set(leftCounter++, obstacles.get(j));
          } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
            rightObstacles.set(rightCounter++, obstacles.get(j));
          } else {
            /* Split obstacle j. */
            let t = Vector2.det(obstacleI2.point_.sub(obstacleI1.point_, _v1),
              obstacleJ1.point_.sub(obstacleI1.point_, _v2)) /
              Vector2.det(obstacleI2.point_.sub(obstacleI1.point_, _v1),
                obstacleJ1.point_.sub(obstacleJ2.point_, _v2));

            let splitPoint = obstacleJ2.point_.sub(obstacleJ1.point_, _v3).mulScalar(t).add(obstacleJ1.point_);

            let newObstacle = Obstacle.pool.get();
            newObstacle.direction_.set(obstacleJ1.direction_);
            newObstacle.point_.set(splitPoint);
            newObstacle.next_ = obstacleJ2;
            newObstacle.previous_ = obstacleJ1;
            newObstacle.id_ = this.simulator_.obstacles_.length;
            newObstacle.isConvex_ = true;
            this.simulator_.obstacles_.push(newObstacle);

            obstacleJ1.next_ = newObstacle;
            obstacleJ2.previous_ = newObstacle;

            if (j1LeftOfI > 0) {
              leftObstacles.set(leftCounter++, obstacleJ1);
              rightObstacles.set(rightCounter++, newObstacle);
            } else {
              rightObstacles.set(rightCounter++, obstacleJ1);
              leftObstacles.set(leftCounter++, newObstacle);
            }
          }
        }
      }

      node.obstacle = obstacleI1;
      node.left = this.buildObstacleTreeRecursive(leftObstacles);
      node.right = this.buildObstacleTreeRecursive(rightObstacles);

      return node;
    }
    return null;
  }

  /**
   * @brief     Computes the agent neighbors of the specified agent.
   * @param[in] agent        A pointer to the agent for which agent neighbors
   *                         are to be computed.
   * @param[in, out] rangeSq The squared range around the agent.
   */
  computeAgentNeighbors(agent: Agent, rangeSq: number) {
    this.queryAgentTreeRecursive(agent, rangeSq, 0);
  }

  /**
   * @brief     Computes the obstacle neighbors of the specified agent.
   * @param[in] agent   A pointer to the agent for which obstacle neighbors are
   *                    to be computed.
   * @param[in] rangeSq The squared range around the agent.
   */
  computeObstacleNeighbors(agent: Agent, rangeSq: number) {
    this.queryObstacleTreeRecursive(agent, rangeSq, this.obstacleTree_);
  }

  /**
   * @brief     Deletes the specified obstacle tree node.
   * @param[in] node A pointer to the obstacle tree node to be deleted.
   */
  deleteObstacleTree(node: ObstacleTreeNode) {
    if (node) {
      this.deleteObstacleTree(node.left);
      this.deleteObstacleTree(node.right);
      node.obstacle = null;
    }
  }

  /**
   * @brief         Recursive function to compute the neighbors of the specified
   *                agent.
   * @param[in]     agent   A pointer to the agent for which neighbors are to be
   *                        computed.
   * @param[in,out] rangeSq The squared range around the agent.
   * @param[in]     node    The current agent k-D tree node.
   */
  queryAgentTreeRecursive(agent: Agent, rangeSq: number, node: number) {
    let agentTree = this.agentTree_.get(node);
    if (agentTree.end - agentTree.begin <= RVO_MAX_LEAF_SIZE) {
      for (let i = agentTree.begin; i < agentTree.end; ++i) {
        agent.insertAgentNeighbor(this.agents_.get(i), rangeSq);
      }
    } else {
      let distLeftMinX = Math.max(0, this.agentTree_.get(agentTree.left).minX - agent.position_.x_);
      let distLeftMaxX = Math.max(0, agent.position_.x_ - this.agentTree_.get(agentTree.left).maxX);
      let distLeftMinY = Math.max(0, this.agentTree_.get(agentTree.left).minY - agent.position_.y_);
      let distLeftMaxY = Math.max(0, agent.position_.y_ - this.agentTree_.get(agentTree.left).maxY);

      let distSqLeft =
        distLeftMinX * distLeftMinX + distLeftMaxX * distLeftMaxX +
        distLeftMinY * distLeftMinY + distLeftMaxY * distLeftMaxY;

      let distRightMinX = Math.max(0, this.agentTree_.get(agentTree.right).minX - agent.position_.x_);
      let distRightMaxX = Math.max(0, agent.position_.x_ - this.agentTree_.get(agentTree.right).maxX);
      let distRightMinY = Math.max(0, this.agentTree_.get(agentTree.right).minY - agent.position_.y_);
      let distRightMaxY = Math.max(0, agent.position_.y_ - this.agentTree_.get(agentTree.right).maxY);

      let distSqRight =
        distRightMinX * distRightMinX + distRightMaxX * distRightMaxX +
        distRightMinY * distRightMinY + distRightMaxY * distRightMaxY;

      if (distSqLeft < distSqRight) {
        if (distSqLeft < rangeSq) {
          this.queryAgentTreeRecursive(agent, rangeSq, agentTree.left);

          if (distSqRight < rangeSq) {
            this.queryAgentTreeRecursive(agent, rangeSq, agentTree.right);
          }
        }
      } else if (distSqRight < rangeSq) {
        this.queryAgentTreeRecursive(agent, rangeSq, agentTree.right);

        if (distSqLeft < rangeSq) {
          this.queryAgentTreeRecursive(agent, rangeSq, agentTree.left);
        }
      }
    }
  }

  /**
   * @brief         Recursive function to compute the neighbors of the specified
   *                obstacle.
   * @param[in]     agent   A pointer to the agent for which neighbors are to be
   *                        computed.
   * @param[in,out] rangeSq The squared range around the agent.
   * @param[in]     node    The current obstacle k-D tree node.
   */
  queryObstacleTreeRecursive(agent: Agent, rangeSq: number, node: ObstacleTreeNode) {
    if (node) {
      let obstacle1 = node.obstacle;
      let obstacle2 = obstacle1.next_;

      let agentLeftOfLine = Vector2.leftOf(obstacle1.point_, obstacle2.point_, agent.position_);

      this.queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.left : node.right);

      let distSqLine = agentLeftOfLine * agentLeftOfLine /
        obstacle2.point_.sub(obstacle1.point_, _v1).absSq();

      if (distSqLine < rangeSq) {
        if (agentLeftOfLine < 0) {
          /* Try obstacle at this node only if agent is on right side of obstacle
           * and can see obstacle. */
          agent.insertObstacleNeighbor(node.obstacle, rangeSq);
        }

        /* Try other side of line. */
        this.queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0 ? node.right : node.left);
      }
    }
  }

  /**
   * @brief     Queries the visibility between two points within a specified
   *            radius.
   * @param[in] vector1 The first point between which visibility is to be
   * tested.
   * @param[in] vector2 The second point between which visibility is to be
   *                    tested.
   * @param[in] radius  The radius within which visibility is to be tested.
   * @return    True if q1 and q2 are mutually visible within the radius; false
   *            otherwise.
   */
  queryVisibility(vector1: Vector2, vector2: Vector2, radius: number) {
    return this.queryVisibilityRecursive(vector1, vector2, radius, this.obstacleTree_);
  }

  /**
   * @brief     Recursive function to query the visibility between two points
   *            within a specified radius.
   * @param[in] vector1 The first point between which visibility is to be
   *                    tested.
   * @param[in] vector2 The second point between which visibility is to be
   *                    tested.
   * @param[in] radius  The radius within which visibility is to be tested.
   * @param[in] node    The current obstacle k-D tree node.
   * @return    True if q1 and q2 are mutually visible within the radius; false
   *            otherwise.
   */
  queryVisibilityRecursive(vector1: Vector2, vector2: Vector2, radius: number, node: ObstacleTreeNode) {
    if (node) {
      let obstacle1 = node.obstacle;
      let obstacle2 = obstacle1.next_;

      let q1LeftOfI =
        Vector2.leftOf(obstacle1.point_, obstacle2.point_, vector1);
      let q2LeftOfI =
        Vector2.leftOf(obstacle1.point_, obstacle2.point_, vector2);
      let invLengthI =
        1 / obstacle2.point_.sub(obstacle1.point_, _v1).absSq();

      if (q1LeftOfI >= 0 && q2LeftOfI >= 0) {
        return this.queryVisibilityRecursive(vector1, vector2, radius, node.left) &&
          ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius &&
            q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) ||
            this.queryVisibilityRecursive(vector1, vector2, radius, node.right));
      }

      if (q1LeftOfI <= 0 && q2LeftOfI <= 0) {
        return this.queryVisibilityRecursive(vector1, vector2, radius, node.right) &&
          ((q1LeftOfI * q1LeftOfI * invLengthI >= radius * radius &&
            q2LeftOfI * q2LeftOfI * invLengthI >= radius * radius) ||
            this.queryVisibilityRecursive(vector1, vector2, radius, node.left));
      }

      if (q1LeftOfI >= 0 && q2LeftOfI <= 0) {
        /* One can see through obstacle from left to right. */
        return this.queryVisibilityRecursive(vector1, vector2, radius, node.left) &&
          this.queryVisibilityRecursive(vector1, vector2, radius, node.right);
      }

      let point1LeftOfQ = Vector2.leftOf(vector1, vector2, obstacle1.point_);
      let point2LeftOfQ = Vector2.leftOf(vector1, vector2, obstacle2.point_);
      let invLengthQ = 1 / vector2.sub(vector1).absSq();

      return point1LeftOfQ * point2LeftOfQ >= 0 &&
        point1LeftOfQ * point1LeftOfQ * invLengthQ > radius * radius &&
        point2LeftOfQ * point2LeftOfQ * invLengthQ > radius * radius &&
        this.queryVisibilityRecursive(vector1, vector2, radius, node.left) &&
        this.queryVisibilityRecursive(vector1, vector2, radius, node.right);
    }

    return true;
  }

  agents_: RVOArray<Agent>;
  agentTree_: RVOArray<AgentTreeNode>;
  obstacleTree_: ObstacleTreeNode;
  simulator_: RVOSimulator;
  isAgentsTreeDirty: boolean;
  isObstacleTreeDirty: boolean;
}
